#include "MotorThread.h"
#include "Motor.h"
#include "I2C/I2C.h"

MotorThread::MotorThread(Motor* main, Motor* jib, Motor* portRudder, Motor* stbdRudder){
    _main = main;
    _jib = jib;
    _portRudder = portRudder;
    _stbdRudder = stbdRudder;
}

void MotorThread::run(){
    digitalWrite(44, HIGH);
    //Serial.println("m");

    byte readBuffer[22];
    byte sendBuffer[10];
    size_t checksum = 0;

    //THREAD_BEGIN()

    //Read measured positions from rudder motors
    /*Wire.requestFrom(1, 18);
    _delay_ms(2); //not sure if this is necessary, but it was included in the old code

    checksum = 0;
    for(int8_t i=0; i<16; i++){
        readBuffer[i] = static_cast<byte>(Wire.read());
        checksum += readBuffer[i];
    }
    readBuffer[16] = static_cast<byte>(Wire.read());
    readBuffer[17] = static_cast<byte>(Wire.read());

    size_t checksumTransmitted = static_cast<size_t>(readBuffer[16]) + static_cast<size_t>(readBuffer[17] << 8);
    if(checksum == checksumTransmitted){
        //Leaving out the reading of PWM max and current, for now
        uint32_t portRead = static_cast<uint32_t>(readBuffer[0]) + (static_cast<uint32_t>(readBuffer[1]) << 8) + (static_cast<uint32_t>(readBuffer[2]) << 16) + (static_cast<uint32_t>(readBuffer[3]) << 24);
        uint32_t stbdRead = static_cast<uint32_t>(readBuffer[8]) + (static_cast<uint32_t>(readBuffer[9]) << 8) + (static_cast<uint32_t>(readBuffer[10]) << 16) + (static_cast<uint32_t>(readBuffer[11]) << 24);

        _portRudder->setMeasuredPosition(portRead);
        _stbdRudder->setMeasuredPosition(stbdRead);
    }

    //Read measured positions from sail motors
    Wire.requestFrom(2, 18);
    _delay_ms(2);

    checksum = 0;
    for(int8_t i=0; i<16; i++){
        readBuffer[i] = static_cast<byte>(Wire.read());
        checksum += readBuffer[i];
    }
    readBuffer[16] = static_cast<byte>(Wire.read());
    readBuffer[17] = static_cast<byte>(Wire.read());

    checksumTransmitted = static_cast<size_t>(readBuffer[16]) + static_cast<size_t>(readBuffer[17] << 8);
    if(checksum == checksumTransmitted){
        uint32_t mainRead = static_cast<uint32_t>(readBuffer[0]) + (static_cast<uint32_t>(readBuffer[1]) << 8) + (static_cast<uint32_t>(readBuffer[2]) << 16) + (static_cast<uint32_t>(readBuffer[3]) << 24);
        uint32_t jibRead = static_cast<uint32_t>(readBuffer[8]) + (static_cast<uint32_t>(readBuffer[9]) << 8) + (static_cast<uint32_t>(readBuffer[10]) << 16) + (static_cast<uint32_t>(readBuffer[11]) << 24);

        _main->setMeasuredPosition(mainRead);
        _jib->setMeasuredPosition(jibRead);
    }*/

    I2c.pullup(1);
    I2c.scan();
/*
    //Send positions to rudders
    Wire.beginTransmission(1);
    //I2c.begin();
    checksum = 0;

    sendBuffer[0] = 'g';
    checksum += static_cast<size_t>(sendBuffer[0]);

    //byte buf[32];
    //itoa(_portRudder->getCommandedPositionRaw(), buf, 10);
    //Serial.println(buf);

    //each of these loops only send 2 chars, but our positions are stored as 32-bit integers (as per the old code)
    //need to investigate whether or not 32-bit precision is actually needed or not
    for(uint8_t i=1; i<3; i++){
        sendBuffer[i] = static_cast<byte>(_portRudder->getCommandedPositionRaw() >> ((i-1)*8));
        checksum += static_cast<size_t>(sendBuffer[i]);
    }
    for(uint8_t i=3; i<5; i++){
        sendBuffer[i] = static_cast<byte>(_stbdRudder->getCommandedPositionRaw() >> ((i-3)*8));
        checksum += static_cast<size_t>(sendBuffer[i]);
    }
    for(uint8_t i=5; i<7; i++){
        sendBuffer[i] = static_cast<byte>(checksum >> ((i-5)*8));
    }

    Wire.write(sendBuffer, 7);
    Wire.endTransmission();
    //uint8_t r = I2c.write(0x1, 'g', sendBuffer);

    //byte dbg[20];
    //itoa(r, dbg, 10);
    //Serial.write(dbg);
    //I2c.end();

    _delay_ms(200);

    //Send positions to winches
    Wire.beginTransmission(2);
    //I2c.begin();
    checksum = 0;

    sendBuffer[0] = 'g';
    checksum += static_cast<size_t>(sendBuffer[0]);

    //ditto above with regards to bit precision
    for(uint8_t i=1; i<3; i++){
        sendBuffer[i] = _main->getCommandedPositionRaw() >> ((i-1)*8);
        checksum += static_cast<size_t>(sendBuffer[i]);
    }
    for(uint8_t i=3; i<5; i++){
        sendBuffer[i] = _jib->getCommandedPositionRaw() >> ((i-3)*8);
        checksum += static_cast<size_t>(sendBuffer[i]);
    }
    for(uint8_t i=5; i<7; i++){
        sendBuffer[i] = checksum >> ((i-5)*8);
    }

    Wire.write(sendBuffer, 7);
    Wire.endTransmission();
    //I2c.write(0x2, 'g', sendBuffer);
    //I2c.end();

    //THREAD_END()

    digitalWrite(44, LOW);
    */
}
